COMMENT ⊗   VALID 00005 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	ENTRY
C00007 00003	 PQINIT computes several quantities depending on the order of the
C00008 00004	 DISCAL uses the reference positions X and Y and the distorted
C00022 00005	 DISREM uses the distortion calibration coefficients G as computed by
C00026 ENDMK
C⊗;
ENTRY;
BEGIN "DISTOR"

REQUIRE "DUBBLE.SAI[NUM,HPM]" SOURCE_FILE;
DEFINE MMMM="2";
REQUIRE "ACCEL.SAI[1,DBG]" SOURCE_FILE;
DEFINE CRLF="'15&'12";

REAL XLO,YLO,XHI,YHI,SWW,SH;
INTEGER PP,PH,MG,MR,M1,M2,M3,M4,M;
INTEGER I,J,K;

COMMENT  DMODEL uses the values of X and Y to compute the coefficients of
	 the parameters and inserts them into the A and B arrays, which
	 can be used to obtain the corrections in X and Y, respectively;
SIMPLE PROCEDURE DMODEL (REAL ARRAY A,B; REAL X,Y);
   BEGIN
   REAL XX,XY,YY,YDX,RR,RRX,RRY;
   IF X=0 THEN YDX←0 ELSE YDX←Y/X;
   I←1;
   A[1]←B[M1]←XX←1;
   YY←1;
   FOR J←1 STEP 1 UNTIL PP DO
      BEGIN
      I←I+1;
      XX←XX*X;
      YY←YY*Y;
      A[I]←B[MG+I]←XY←XX;
      FOR K←1 STEP 1 UNTIL J-1 DO
	 BEGIN
      	 I←I+1;
       	 A[I]←B[MG+I]←XY←XY*YDX;
         END;
      I←I+1;
      A[I]←B[MG+I]←YY;
      END;
   IF MR>0 THEN
      BEGIN
      RR ← X↑2 + Y↑2;
      XY←RR↑PH;
      RRX←X*XY;
      RRY←Y*XY;
      A[M3]←RRX;
      B[M3]←RRY;
      FOR I←M4 STEP 1 UNTIL M DO
         BEGIN
         A[I] ← RRX ← RRX*RR;
         B[I] ← RRY ← RRY*RR;
         END;
      END;
   END;
COMMENT  PQINIT computes several quantities depending on the order of the
	 polynominals and needed in subsequent calculations;
INTERNAL PROCEDURE PQINIT (INTEGER P,Q);
   BEGIN
   PP←P;
   PH ← (P+1) DIV 2;
   MG ← (P+1)*(P+2)%2;
   MR ← 0 MAX ((Q+1) DIV 2 - PH);
   M1←MG+1;
   M2←2*MG;
   M3←M2+1;
   M4←M3+1;
   M←M2+MR
   END;
COMMENT  DISCAL uses the reference positions X and Y and the distorted
	 positions XP and YP of N image points to compute the distortion
	 calibration polynomial coefficients G (for converting X,Y to XP,YP)
	 and their covariance matrix S.  (To allow P and Q to have their
	 maximum values, G should be dimensioned [1:44] and S should be
	 dimensioned [1:44,1:44].)  P is the degree of the two-dimensional
	 polynomials for general distortion, and Q is the degree of the
	 one-dimensional polynomial for radial distortion.  (An even value
	 of Q is equivalent to the next lower odd value.  All values of Q
	 such that Q≤P are equivalent.)
	 SD is the computed standard deviation of the observation errors (unmodeled
	 errors in X and Y).  If on input 0≤P≤5 and Q≤10, these values are
	 accepted.  Otherwise, typed-in values P and Q are asked for, with
	 the entire process repeating until only a carriage return is
	 typed for P.  The final values are returned.;

INTERNAL PROCEDURE DISCAL (INTEGER N; REFERENCE INTEGER P,Q;
	REAL ARRAY X,Y,XP,YP,G,S; REFERENCE REAL SD);
   BEGIN
   REAL ARRAY A,B,C,CP[1:44],H[1:44,1:44],XD,YD,XR,YR[1:N],DT[1:2],DC[1:44,1:2],
           DH[1:990,1:2];
   REAL R,XX,YY,ACC,SD2;
   INTEGER PNT,NUM;
   BOOLEAN FLAG;

   FOR PNT←1 STEP 1 UNTIL N DO
      BEGIN
      XD[PNT] ← XP[PNT]-X[PNT];
      YD[PNT] ← YP[PNT]-Y[PNT]
      END;

   IF P≥0 ∧ P≤5 ∧ Q≤10 THEN
      BEGIN "MAIN LOOP"
      PQINIT(P,Q);
      FOR I←1 STEP 1 UNTIL M DO DC[I,1]←DC[I,2]←0;
      K ← M*(M+1)%2;
      FOR I←1 STEP 1 UNTIL K DO DH[I,1]←DH[I,2]←0;
      FOR PNT←1 STEP 1 UNTIL N DO
         BEGIN
         DMODEL(A,B,X[PNT],Y[PNT]);
         FOR I←1 STEP 1 UNTIL MG, M3 STEP 1 UNTIL M DO
            BEGIN
            MKD(DT[1],A[I]*XD[PNT]);
            DAD(DC[I,1],DT[1]);
            K ← I*(I-1)%2;
            FOR J←1 STEP 1 UNTIL MG MIN I, M3 STEP 1 UNTIL I DO
               BEGIN
               MKD(DT[1],A[I]*A[J]);
               DAD(DH[K+J,1],DT[1])
               END;
            END;
         FOR I←M1 STEP 1 UNTIL M DO
            BEGIN
            MKD(DT[1],B[I]*YD[PNT]);
            DAD(DC[I,1],DT[1]);
            K ← I*(I-1)%2;
            FOR J←M1 STEP 1 UNTIL I DO
               BEGIN
               MKD(DT[1],B[I]*B[J]);
               DAD(DH[K+J,1],DT[1])
               END
            END
         END;
      FOR I←1 STEP 1 UNTIL M DO C[I]←DC[I,1];
      K←0;
      FOR I←1 STEP 1 UNTIL M DO FOR J←1 STEP 1 UNTIL I DO
         BEGIN
         K←K+1;
         H[I,J] ← H[J,I] ← DH[K,1];
         END;

      INVERT(M,S,H,FLAG);
      IF FLAG THEN OUTSTR("SINGULAR H MATRIX IN DISCAL" & CRLF);
      TRANS(M,G,S,C);
      TRANS(M,CP,H,G);
      ACC←R←0;
      FOR I←1 STEP 1 UNTIL M DO
         BEGIN
         R ← R + C[I]↑2;
         ACC ← ACC + (CP[I]-C[I])↑2
         END;
      ACC←SQRT(ACC/R);
      R←0;
      FOR PNT←1 STEP 1 UNTIL N DO
         BEGIN
         DMODEL(A,B,X[PNT],Y[PNT]);
         XX←0;
         FOR I←1 STEP 1 UNTIL MG, M3 STEP 1 UNTIL M DO
                 XX ← XX + A[I]*G[I];
         XR[PNT] ← XD[PNT]-XX;
         YY←0;
         FOR I←M1 STEP 1 UNTIL M DO
                 YY ← YY + B[I]*G[I];
         YR[PNT] ← YD[PNT]-YY;
         R ← R + XR[PNT]↑2 + YR[PNT]↑2;
         END;

      SD2 ← R/(2*N-M);
      SD ← SQRT(SD2);
      FOR I←1 STEP 1 UNTIL M DO FOR J←1 STEP 1 UNTIL M DO S[I,J]←SD2*S[I,J];
      END "MAIN LOOP";

   P←PP;
   END "DISCAL";
COMMENT  DISREM uses the distortion calibration coefficients G as computed by
	 DISCAL to convert the N points XI,YI into the N points XO,YO.  (XO and YO
	 can refer to the same arrays as XI and YI.)  If TOL<0, the forward
	 conversion is done (X,Y to XP,YP in DISCAL).  If TOL≥0, the inverse
	 conversion is done (XP,YP to X,Y in DISCAL), and TOL is the convergence
	 tolerance (in XO,YO) for terminating the iterations (with a maximum
	 of 10 iterations).  (PQINIT must be called before this procedure to
	 compute the functions of P and Q, unless this has already been
	 done by calling DISCAL);
INTERNAL SIMPLE PROCEDURE DISREM (INTEGER N; REAL TOL;
	REAL ARRAY G; REFERENCE REAL XI,YI,XO,YO);
   BEGIN
   OWN REAL ARRAY A,B[1:44];
   SAFE OWN REAL ARRAY XY,D[1:2];
   REAL XX,YY,TOL2;
   INTEGER PNT,ITER;
   TOL2←TOL↑2;
   FOR PNT←1 STEP 1 UNTIL N DO
      BEGIN "POINTS"
      XY[1]←MEMORY[LOCATION(XI)+PNT-1,REAL];
      XY[2]←MEMORY[LOCATION(YI)+PNT-1,REAL];
      FOR ITER←1 STEP 1 UNTIL 10 DO
         BEGIN
         DMODEL(A,B,XY[1],XY[2]);
         XX←0;
         FOR I←1 STEP 1 UNTIL MG, M3 STEP 1 UNTIL M DO
                 XX ← XX + A[I]*G[I];
         YY←0;
         FOR I←M1 STEP 1 UNTIL M DO
                 YY ← YY + B[I]*G[I];
         IF TOL<0 THEN
            BEGIN
            MEMORY[LOCATION(XO)+PNT-1,REAL] ← MEMORY[LOCATION(XI)+PNT-1,REAL]+XX;
            MEMORY[LOCATION(YO)+PNT-1,REAL] ← MEMORY[LOCATION(YI)+PNT-1,REAL]+YY;
            CONTINUE "POINTS";
            END
         ELSE
            BEGIN
            XX ← MEMORY[LOCATION(XI)+PNT-1,REAL]-XX;
            YY ← MEMORY[LOCATION(YI)+PNT-1,REAL]-YY;
            D[1] ← XX-XY[1];
            D[2] ← YY-XY[2];
            ACCELERATE(2,XY,D,ITER=1,K);
            END;
         IF (D[1]↑2+D[2]↑2)<TOL2 THEN DONE;
         END;
      MEMORY[LOCATION(XO)+PNT-1,REAL]←XY[1];
      MEMORY[LOCATION(YO)+PNT-1,REAL]←XY[2];
      END "POINTS";
   END "DISREM";

END "DISTOR";